/**
 * @file MSP430_AHRS_main.c
 * @mainpage MSP430 AHRS 9-Axis Sensor Fusion
 *
 * @detail Description:
 * Attitude and Heading Reference System Software Implementation using the
 * Direction Cosine Matrix Algorithm on the MSP430F5xx Family.
 * ACLK = REFO, MCLK = SMCLK = 16 MHz
 *
 * @author E. Macias / D. Torres / S. Ravindran
 * @author Texas Instruments, Inc
 * @date December, 2011
 * @version 1.0 - Initial version
 * @note Built with IAR Embedded Workbench: 5.20.1 and CCS Version 5.1.0.09000
 **/

// Based on:
// Mongoose v1.0 9DoF IMU + Barometric pressure sensor AHRS demo
// www.ckdevices.com
// 9 Degree of Freedom Attitude and Heading Reference System
// Firmware v1.0
//
// Released under Creative Commons License
// Modifications and additions by Cory Duce to allow it to work with Mongoose hardware
// Based on code by Doug Weibel and Jose Julio which was based on ArduIMU v1.5 by Jordi Munoz and William Premerlani, Jose Julio and Doug Weibel
//

// Axis definition:
   // X axis pointing forward (to the JTAG connector)
   // Y axis pointing to the right
   // Z axis pointing down.
// Positive pitch : nose up
// Positive roll : right wing down
// Positive yaw : clockwise


#include "main.h"

// Initialize Global variables

int16_t SENSOR_SIGN[9] = { 1,1,1,1,1,1,1,1,1};  //Correct directions x,y,z - gyros, accels, magnetormeter

//Structure for holding raw and calibration corrected data from the sensors
struct s_sensor_data sen_data = {0,0,0, // Gyro Raw x,y,z
				 0,0,0, // Accelerometer Raw x,y,z
				 0,0,0, // Magnetometer Raw x,y,z

    				 // Corrected based on calibration values
    				 0,0,0, // Gyro x,y,z
    				 0,0,0, // Accelerometer x,y,z
				 0,0,0, // Magnetometer x,y,z
  				 0, // Magnetometer heading
  				 0, // Barometer Temperature
  				 0};// Barometer Pressure

struct s_sensor_offsets sen_offset = {{0,0,0}, // Gyro offset
				      {0,0,0}, // Accelerometer offset
				      {0,0,0}, // Magnetometer offset
				       0, // Magnetometer XY Theta
				       0, // Magnetometer XY Scale
				       0, // Magnetometer YZ Theta
				       0};// Magnetometer YZ Scale

uint8_t global_tx_buffer[32];

float Accel_Vector[3];    //Store the acceleration in a vector
float Mag_Vector[3];      //Store the magnetometer direction in a vector
float Gyro_Vector[3];     //Store the gyros turn rate in a vector
float Omega_Vector[3];    //Corrected Gyro_Vector data
float Omega_P[3];         //Omega Proportional correction
float Omega_I[3];         //Omega Integrator
float Omega[3];

// Euler angles
float roll;
float pitch;
float yaw;

float errorRollPitch[3];
float errorYaw[3];


float DCM_Matrix[3][3];
float Update_Matrix[3][3];
float Temporary_Matrix[3][3];

// Local Variables
// @brief tx_packet_length is how many bytes of data will be written
uint8_t tx_packet_length;
// @brief *tx_packet_data will point to the data to be written
uint8_t *tx_packet_data;

// @brief rx_packet_length is how many bytes of data will be read
uint8_t rx_packet_length;
// @brief *rx_packet_data will point to the data to be read
uint8_t *rx_packet_data;

// @brief receive flag is used for the ISR to know the MSP430 will be reading from the
// CP
uint8_t receive_flag;
// @brief transmit flag is used for the ISR to know the MSP430 will be writing to the
// CP
uint8_t transmit_flag;

// @brief Used to keep track how many bytes have been received
uint8_t rx_byte_ctr;
// @brief Used to keep track how many bytes have been transmitted
uint8_t tx_byte_ctr;

void Init_Sensors(void);
void Print_Data(float x_value, float y_value, float z_value);

void main(void)
{
#ifdef CALIBRATION_MODE
   uint8_t Incoming_command;
   uint16_t Sensor_Samples_Requested;
#endif

   // Initialize MSP430 Clock, Pin Peripherals, I2C Module
   Init_MCU();

   // Initialize Sensors
   Init_Sensors();

  __disable_interrupt();                       //Disable interrupts globally


   // For-loop runs every 20 mS (50 Hz)
   for (;;)
    {
      // Read motion sensors
       Read_Gyro();
       Read_Accelerometer();
       Read_Compass();    // Read magnetometer

        __bis_SR_register(LPM0_bits + GIE);                             //Enter LPM0 until awakened by an event handler
    }//for(;;)

}

/**
* @brief <b>Function Name</b>:     : Init_Sensors
* @brief  <b>Description</b>: This function initializes the accelerometer,
* magnetometer, gyroscope, and offsets value for each sensor.
* @param Input Parameters: None
* @return Return Values: None
**/
void Init_Sensors(void)
{
   Init_Accelerometer();
   Init_Compass();
   Init_Gyro();
   calibrateSensors();
}

/**
* @brief <b>Function Name</b>:     : Print_Data
* @brief  <b>Description</b>: This function sends the GUI float values x, y,
* and z with a ";" in between each value. Also it sends a end of packet delimiter
* "!" via USB.
* @param Input Parameters: None
* @return Return Values: None
**/
void Print_Data(float x_value, float y_value, float z_value)
{

}
